package utility;


//TODO find inverse matrix transform method
public class Matrix4x4 extends Matrix {

	public Matrix4x4(double[][] elements) {
		super(validate(elements));
	}

	private static double[][] validate(double[][] elements) {
		if(elements.length != 4) {
			throw new IllegalArgumentException("Matrix must have 4 rows.");
		}
		for(int i = 0; i < elements.length; i++) {
			if(elements[i] == null || elements[i].length != 4) {
				throw new IllegalArgumentException("Matrix must have 4 columns.");
			}
		}
		return elements;
	}
	
	public Vector3 multiply(Vector3 vector) {
		double[] elements = new double[4];
		for(int i = 0; i < vector.elements.length; i++) {
			elements[i] = vector.elements[i];
		}
		elements[3] = 1;
		Vector homog = new Vector(elements);
		homog = this.multiply(homog);
		double[] nonHomog = new double[3];
		for(int i = 0; i < nonHomog.length; i++) {
			nonHomog[i] = homog.elements[i] / homog.elements[3];
		}
		return new Vector3(nonHomog);
	}
	
	public Matrix4x4 multiply(Matrix4x4 other) {
		double[][] product = ((Matrix)this).multiply(other).elements;
		return new Matrix4x4(product);
	}
	
	public static Matrix4x4 identity() {
		double[][] elements = new double[][]{{1, 0, 0, 0},
											 {0, 1, 0, 0},
											 {0, 0, 1, 0},
											 {0, 0, 0, 1}
		};
		return new Matrix4x4(elements);
	}
	
	public static Matrix4x4 getInverseScaleMatrix(double x, double y, double z) {
		double[][] scale = new double[][]{{ 1.0 / x, 0, 0, 0 },
										  { 0, 1.0 / y, 0, 0},
										  { 0, 0, 1.0 / z, 0},
										  { 0, 0, 0, 1}};
		return new Matrix4x4(scale);								    
	}
	
	public static Matrix4x4 getRotationMatrix(double x, double y, double z) {
		return getRotationMatrix(x, y, z, false);
	}
	
	public static Matrix4x4 getInverseRotationMatrix(double x, double y, double z) {
		return getRotationMatrix(x, y, z, true);
	}
	
	private static Matrix4x4 getRotationMatrix(double x, double y, double z, boolean inverse) {
		double sign = (inverse) ? -1 : 1;
		Matrix4x4 rotateX = new Matrix4x4(new double[][]{
				{1, 0,             0,            0},
				{0, Math.cos(sign * x), -Math.sin(sign * x), 0},
				{0, Math.sin(sign * x),  Math.cos(sign * x), 0},
				{0, 0,             0,            1}
		});
		Matrix4x4 rotateY = new Matrix4x4(new double[][]{
				{ Math.cos(sign * y), 0, Math.sin(sign * y), 0},
				{ 0,            1, 0,            0},
				{-Math.sin(sign * y), 0, Math.cos(sign * y), 0},
				{ 0,            0, 0,            1} 
		});
		Matrix4x4 rotateZ = new Matrix4x4(new double[][]{
				{Math.cos(sign * z), -Math.sin(sign * z), 0, 0},
				{Math.sin(sign * z),  Math.cos(sign * z), 0, 0},
				{0,             0,            1, 0},
				{0,             0,            0, 1}
		});
		Matrix4x4 rotation = rotateZ.multiply(rotateY).multiply(rotateX);
		return rotation;
	}
	
	public static Matrix4x4 getInverseTranslationMatrix(double x, double y, double z){
		double[][] translateM = new double[][] {{ 1, 0, 0, -x},
											    { 0, 1, 0, -y},
											    { 0, 0, 1, -z},
											    { 0, 0, 0, 1}};
		return new Matrix4x4(translateM);
	}
	
	public static Matrix4x4 getRotationMatrix(Vector3 axis, double angle) {
		return getRotationMatrix(axis, angle, false);
	}
	
	public static Matrix4x4 getInverseRotationMatrix(Vector3 axis, double angle) {
		return getRotationMatrix(axis, angle, true);
	}
	
	/*
	 * https://www.matterhackers.com/static/docs/mattercad/MatterHackers.VectorMath/Matrix4X4.htm#CreateFromAxisAngle
	 */
	private static Matrix4x4 getRotationMatrix(Vector3 axis, double angle, boolean inverse) {
		double sign = (inverse) ? 1 : -1;
        axis.normalize();
        double cos = Math.cos(sign * angle);
        double sin = Math.sin(sign * angle);
        double t = 1.0 - cos;
        double x = axis.x();
        double y = axis.y();
        double z = axis.z();

        double[][] matrix = new double[][]{
        		{t * x * x + cos, t * x * y - sin * z, t * x * z + sin * y, 0.0},
        		{t * x * y + sin * z, t * y * y + cos, t * y * z - sin * x, 0.0},
        		{t * x * z - sin * y, t * y * z + sin * x, t * z * z + cos, 0.0},
        		{0, 0, 0, 1}};
		return new Matrix4x4(matrix);
	}
	
	public static Matrix4x4 getScaleMatrix(double x, double y, double z) {
		double[][] scale = new double[][]{{ x, 0, 0, 0 },
										  { 0, y, 0, 0},
										  { 0, 0, z, 0},
										  { 0, 0, 0, 1}};
		return new Matrix4x4(scale);								    
	}
	
	public static Matrix4x4 getTranslationMatrix(double x, double y, double z){
		double[][] translateM = new double[][] {{ 1, 0, 0, x},
											    { 0, 1, 0, y},
											    { 0, 0, 1, z},
											    { 0, 0, 0, 1}};
		return new Matrix4x4(translateM);
	}
}
